#include <robot/robot.h>
#include <robot/nao/cnaoclient.h>

#include <buola/app/ccmdline.h>
#include <buola/tty/csimpleshell.h>

using namespace buola;
using namespace buola::robot;

static std::unique_ptr<nao::CNaoClient> sClient;
static std::unique_ptr<tty::CSimpleShell> sShell;

void cmd_pos(const std::vector<std::string> &pArgs)
{
    if(pArgs.size()!=27)
        throw XArgument("wrong number of parameters");
    
    mat::CVecNd<26> lJoints;
    for(int i=0;i<26;i++)
        lJoints[i]=to_<double>(pArgs[i+1]);
    sClient->SetGoal(lJoints);
}

void cmd_info(const std::vector<std::string> &pArgs)
{
    mat::CVecNd<26> lJoints;

    sClient->GetPose(lJoints);

    msg_info() << "joints=";
    for(int i=0;i<26;i++)
    {
        if(i) msg_info() << ',';
        msg_info() << rad2deg(lJoints[i]);
    }
    msg_info() << "\n";
}

void cmd_speak(const std::vector<std::string> &pArgs)
{
    sClient->Speak();
}

void cmd_walk(const std::vector<std::string> &pArgs)
{
    sClient->Walk();
}

void cmd_posture(const std::vector<std::string> &pArgs)
{
    if(pArgs.size()!=2)
        throw XArgument("wrong number of parameters");
    sClient->Posture(pArgs[1]);
}

void cmd_relax(const std::vector<std::string> &pArgs)
{
    sClient->Relax();
}

void cmd_quit(const std::vector<std::string> &pArgs)
{
    sShell->Stop();
}

int main(int pNArgs,char **pArgs)
{
    buola_init(pNArgs,pArgs);
    
    try
    {
        sClient.reset(new nao::CNaoClient);

        if(cmd_line().ArgCount())
        {
            sClient->Init(blackbox::CClientProxy(cmd_line().Arg(0))["nao"]);
        }
        else
        {
            sClient->Init();
        }

        sShell.reset(new tty::CSimpleShell);
        sShell->AddCommand("pos",cmd_pos);
        sShell->AddCommand("info",cmd_info);
        sShell->AddCommand("speak",cmd_speak);
        sShell->AddCommand("walk",cmd_walk);
        sShell->AddCommand("relax",cmd_relax);
        sShell->AddCommand("posture",cmd_posture);
        sShell->AddCommand("quit",cmd_quit);
        
        sShell->Loop();
    }
    catch(std::exception &pE)
    {
        msg_error() << "caught exception:" << pE.what() << "\n";
    }

    return buola_finish();
}
